#include "outline/simplify.h"
#include "outline/type_traits.h"
#include "glib/g2/smatrix.h"
#include <limits>

namespace outline {

///node_here[i] describes what would be if destination outline node is in i'th source outline node
struct node_here {
	double d; ///< metrics of approximation discrepancy (-1, if no node is possible here)
	unsigned prev_node; ///< best previous node is coincide with #prev_node source outline node (0, if this step is the first one)

	node_here() { }
	node_here(double dd, unsigned pn) : d(dd), prev_node(pn) { }
};

///The class accumulates information about start, several intermediate, and finish points,
///and provides summed square of discrepancy of intermediate points from line start-finish.
///The class allows easy testing of new finish point (previous finish point becomes else one intermediate point).
template <typename A>
class best_line {
public:
	typedef A accumulator_type;
	typedef g2::basic_vector<accumulator_type>	vector;
	typedef g2::basic_smatrix<accumulator_type>	smatrix;
public:
	best_line() { }
	best_line(vector const & s)
		{ start(s); }
public:
	///forgets accumulated information and sets new start point of line
	void start(vector const & s)
		{ r0 = s; q.zero(); }
	///what discrepancy would be if finish is in given point?
	double finish(vector const & f) const
		{ return measure(tsquare(f - r0)); }
	///accumulates information about else one intermediate point
	void intermediate(vector const & i)
		{ q += tsquare(i - r0); }
	///the same as sequence of finish(...) value folloed by intermediate(...)
	double intermediate_measure(vector const & i) { 
		smatrix sqr_delta = tsquare(i - r0);
		double r = measure(sqr_delta);
		//assert (r >= min_discrepancy());
		q += sqr_delta;
		return r;
	}
	///wherever final point is, discrepancy will not be less than returned value
	double min_discrepancy() const
		{ return g2::smatrix(q).eigenvalue(0); }
	///measures distances from start point (not from any line)
	double discrepancy_from_start() const
		{ return q.tr(); }
private:
	double measure(smatrix const & sqr_delta) const
		{ return q.tr() - double(dscalar_product(q, sqr_delta)) / sqr_delta.tr(); }
private:
	vector r0;
	smatrix q;
};

template <typename T>
static double simplify_min_step_len_core(int n, ///< min_step_len
	std::vector< point<T> > const & src, std::vector< point<T> > & dst, bool closed)
{
	assert(n > 1);

	typedef type_traits<T>::accumulator_type accumulator_type;
	typedef std::vector<node_here> nodes;

	size_t const m = src.size();

	nodes ns(m);

	ns[0].d = 0;
	ns[0].prev_node = 0;

	size_t i;
	best_line<accumulator_type> line;
	line.start(src[0]);
	for (i = 1; i < n && i < m; ++i) {
		ns[i].d = -1;
		ns[i].prev_node = 0;
		line.intermediate(src[i]);
	}
	for (; i < 2*n && i < m; ++i) {
		ns[i].d = line.intermediate_measure(src[i]);
		ns[i].prev_node = 0;
	}
	for (; i < m; ++i) {
		size_t max_j = (i < 3*n) ? i-n+1 : 2*n;
		line.start(src[i]);
		ns[i].d = std::numeric_limits<double>::max();
		size_t j;
		for (j = 1; j < n; ++j)
			line.intermediate(src[i-j]);
		for (; j < max_j; ++j) {
			assert(ns[i-j].d >= 0);
			double d = ns[i-j].d + line.intermediate_measure(src[i-j]);
			if (d < ns[i].d) {
				ns[i].d = d;
				ns[i].prev_node = i - j;

				//test for better start
				if (closed && j == 2*n-1) {
					int s = i - j;
					int c = 1;
					for (int k = 1; k < n; ++k)
						if (ns[i-k].prev_node == s)
							++c;
						else
							break;
					if (c == n) {
						//we found better fixed start point for closed curve
						ns.clear();
						std::vector< point<T> > tmp;
						shift(src, tmp, s);
						return simplify_min_step_len_core(n, tmp, dst, 
							false //no closed flag because second start reselection is unwanted
						);
					}
				}
			}
		}
		assert(ns[i].d != std::numeric_limits<double>::max());
	}

	//back pass
	std::vector< point<T> > tmp;
	tmp.push_back(src[m-1]);
	i = m-1;
	while (i != 0) {
		node_here const & h = ns[i];
		i = h.prev_node;
		tmp.push_back(src[i]);
	}
	dst.assign(tmp.rbegin(), tmp.rend());

	return ns[m-1].d;
}

template <typename T>
double simplify_min_step_len(int min_step_len,
	std::vector< point<T> > const & src, std::vector< point<T> > & dst)
{
	if (min_step_len <= 1) {
		dst = src;
		return 0;
	}

	return simplify_min_step_len_core(min_step_len, src, dst, closed(src));
}

template <typename T>
double simplify_min_step_len(int min_step_len,
	std::vector<std::vector<point<T> > > const & src, std::vector<std::vector<point<T> > > & dst)
{
	dst.resize(src.size());
	double s = 0;
	for (int i = 0; i < src.size(); ++i)
		s += simplify_min_step_len(min_step_len, src[i], dst[i]);
	return s;
}

template double simplify_min_step_len(int min_step_len,
	outlines_us const & src, outlines_us & dst);
template double simplify_min_step_len(int min_step_len,
	outlines_s const & src, outlines_s & dst);
template double simplify_min_step_len(int min_step_len,
	outlines_i const & src, outlines_i & dst);
template double simplify_min_step_len(int min_step_len,
	outlines_f const & src, outlines_f & dst);
template double simplify_min_step_len(int min_step_len,
	outlines_d const & src, outlines_d & dst);

//

template <typename T>
static void make_first_layer(std::vector< point<T> > const & src,
	std::vector<node_here> & first)
{
	typedef type_traits<T>::accumulator_type accumulator_type;
	best_line<accumulator_type> line;
	int i;
	size_t m = first.size();

	first[0].d = -1;
	line.start(src[0]);
	for (i = 1; i < m-1; ++i) {
		first[i].d = line.intermediate_measure(src[i]);
		first[i].prev_node = 0;
	}
	if (closed(src))
		first[m-1].d = line.discrepancy_from_start();
	else
		first[m-1].d = line.intermediate_measure(src[m-1]);
	first[m-1].prev_node = 0;
}

template <typename T>
static void make_next_layer(std::vector< point<T> > const & src,
	std::vector<node_here> const & last, std::vector<node_here> & next)
{
	typedef type_traits<T>::accumulator_type accumulator_type;
	best_line<accumulator_type> line;
	int i, j;

	size_t m = last.size();
	next.resize(m);
	next[0].d = -1;
	for (i = 1; last[i-1].d < 0 && i < m; ++i)
		next[i].d = -1;
	for (; i < m; ++i) {
		line.start(src[i]);
		next[i].d = std::numeric_limits<double>::max();
		for (j = i-1; last[j].d >= 0; --j) {
			double d = last[j].d + line.intermediate_measure(src[j]);
			if (d < next[i].d) {
				next[i].prev_node = j;
				next[i].d = d;
			}
			else if (line.min_discrepancy() > next[i].d)
				break;
		}
		assert(next[i].d != std::numeric_limits<double>::max());
	}
}

///builds optimal coutour in reverse order by passing through layers
template <typename T>
static void layers_back_pass(std::vector< point<T> > const & src,
	std::vector<std::vector<node_here> > const & layers, int last_step,
	std::vector< point<T> > & reverse)
{
	int i = last_step;
	int k = layers.size() - 1;
	while (i != 0) {
		reverse.push_back(src[i]);
		i = layers[k][i].prev_node;
		--k;
	}
	assert(k == -1);
	reverse.push_back(src[0]);
}

template <typename T>
static double simplify_to_K_nodes_core(int K,
	std::vector< point<T> > const & src, std::vector< point<T> > & dst, bool closed)
{ 
	assert (K >= 2);
	size_t const m = src.size();
	assert (m >= 2);

	typedef type_traits<T>::accumulator_type accumulator_type;
	int i, k;
	best_line<accumulator_type> line;

	if (K == 2) {
		//solution is known, we have to calculate discrepancy only
		line.start(src[0]);
		for (i = 1; i < m-1; ++i)
			line.intermediate(src[i]);
		double d = line.finish(src[m-1]);

		std::vector< point<T> > tmp(2);
		tmp[0] = src[0];
		tmp[1] = src[m-1];
		dst.swap(tmp);
		return d;
	}

	typedef std::vector<node_here> nodes;
	std::vector<nodes> layers(K-2);

	layers[0].resize(m);
	make_first_layer(src, layers[0]);

	for (k = 1; k < K-2; ++k) 
		make_next_layer(src, layers[k-1], layers[k]);

	//last step
	int ls;
	double lsd = std::numeric_limits<double>::max();
	line.start(src[m-1]);
	for (i = m-2; layers[K-3][i].d >= 0; --i) {
		double d = layers[K-3][i].d + line.intermediate_measure(src[i]);
		if (d < lsd) {
			ls = i;
			lsd = d;
		}
	}
	assert(lsd != std::numeric_limits<double>::max());

	if (closed && K >= 3) {
		//how many times node is leaved in contour,
		//summed for simplified contours of size 3..K
		std::vector<int> stat(m);
		for (int j = 1; j <= K-3; ++j) {
			if (j == K-3)
				i = ls;
			else
				i = layers[j+1][m-1].prev_node;
			k = j;
			while (i != 0) {
				++stat[i];
				i = layers[k][i].prev_node;
				--k;
			}
			assert(k == -1);
		}
		int s = std::max_element(stat.begin(), stat.end()) - stat.begin();

		//new fixed start point for closed curve is choosen
		stat.clear();
		layers.clear();
		std::vector< point<T> > tmp;
		shift(src, tmp, s);
		return simplify_to_K_nodes_core(K, tmp, dst, 
			false //no closed flag because second start reselection is unwanted
		);
	}

	//back pass
	std::vector< point<T> > tmp;
	tmp.push_back(src[m-1]);
	layers_back_pass(src, layers, ls, tmp);
	dst.assign(tmp.rbegin(), tmp.rend());

	return lsd;
}

template <typename T>
double simplify_to_K_nodes(int K,
	std::vector< point<T> > const & src, std::vector< point<T> > & dst)
{
	if (K <= 1) {
		dst = src;
		return 0;
	}

	return simplify_to_K_nodes_core(K, src, dst, closed(src));
}


template <typename T>
double simplify_avg_step_len(float avg_step_len,
	std::vector<std::vector<point<T> > > const & src, std::vector<std::vector<point<T> > > & dst)
{
	dst.resize(src.size());
	double s = 0;
	for (int i = 0; i < src.size(); ++i) {
		int K = src[i].size() / avg_step_len + 0.5f;
		if (K < 2)
			K = 2;
		s += simplify_to_K_nodes(K, src[i], dst[i]);
	}
	return s;
}

template double simplify_avg_step_len(float avg_step_len,
	outlines_us const & src, outlines_us & dst);
template double simplify_avg_step_len(float avg_step_len,
	outlines_s const & src, outlines_s & dst);
template double simplify_avg_step_len(float avg_step_len,
	outlines_i const & src, outlines_i & dst);
template double simplify_avg_step_len(float avg_step_len,
	outlines_f const & src, outlines_f & dst);
template double simplify_avg_step_len(float avg_step_len,
	outlines_d const & src, outlines_d & dst);

//

template <typename T>
static double simplify_max_discrepancy_core(double max_discrepancy,
	std::vector< point<T> > const & src, std::vector< point<T> > & dst, bool closed)
{
	assert(max_discrepancy > 0);
	size_t const m = src.size();
	assert (m >= 2);

	typedef type_traits<T>::accumulator_type accumulator_type;
	best_line<accumulator_type> line;

	typedef std::vector<node_here> nodes;
	std::vector<nodes> layers(1);
	layers.reserve(m); //no resizing is permitted

	layers[0].resize(m);
	make_first_layer(src, layers[0]);

	int k;
	for (k = 0; layers[k][m-1].d > max_discrepancy; ++k) {
		layers.push_back(nodes());
		make_next_layer(src, layers[k], layers[k+1]);
	}

	if (closed && k >= 1) {
		//how many times node is leaved in contour,
		//summed for simplified contours of arbitrary size
		std::vector<int> stat(m);
		for (int j = 1; j <= k; ++j) {
			int i = layers[j][m-1].prev_node;
			int l = j-1;
			while (i != 0) {
				++stat[i];
				i = layers[l][i].prev_node;
				--l;
			}
			assert(l == -1);
		}
		int s = std::max_element(stat.begin(), stat.end()) - stat.begin();

		//new fixed start point for closed curve is choosen
		stat.clear();
		layers.clear();
		std::vector< point<T> > tmp;
		shift(src, tmp, s);
		return simplify_max_discrepancy_core(max_discrepancy, tmp, dst, 
			false //no closed flag because second start reselection is unwanted
		);
	}

	//back pass
	std::vector< point<T> > tmp;
	layers_back_pass(src, layers, m-1, tmp);
	dst.assign(tmp.rbegin(), tmp.rend());

	return layers[k][m-1].d;
}

template <typename T>
double simplify_max_discrepancy(double max_discrepancy,
	std::vector< point<T> > const & src, std::vector< point<T> > & dst)
{
	if (max_discrepancy == 0) {
		dst = src;
		return 0;
	}
	return simplify_max_discrepancy_core(max_discrepancy, src, dst, closed(src));
}

template <typename T>
double simplify_avg_discrepancy(double avg_discrepancy,
	std::vector<std::vector<point<T> > > const & src, std::vector<std::vector<point<T> > > & dst)
{
	dst.resize(src.size());
	double s = 0;
	for (int i = 0; i < src.size(); ++i)
		s += simplify_max_discrepancy(src[i].size() * avg_discrepancy, src[i], dst[i]);
	return s;
}

template double simplify_avg_discrepancy(double avg_discrepancy,
	outlines_us const & src, outlines_us & dst);
template double simplify_avg_discrepancy(double avg_discrepancy,
	outlines_s const & src, outlines_s & dst);
template double simplify_avg_discrepancy(double avg_discrepancy,
	outlines_i const & src, outlines_i & dst);
template double simplify_avg_discrepancy(double avg_discrepancy,
	outlines_f const & src, outlines_f & dst);
template double simplify_avg_discrepancy(double avg_discrepancy,
	outlines_d const & src, outlines_d & dst);

} //namespace outline
